#include <buola/buola.h>

#include <buola/image/crowsizzler.h>

#include <xmmintrin.h>

namespace buola { namespace img {

void CRowSizzler::Sizzle(const float *pSrc,float *pDst) const
{
    int lWidth=mPart;
    if(is_sse_aligned(pSrc)&&lWidth%4==0&&mJump%4==0)
    {
        while(lWidth>0)
        {
            __m128 l0=_mm_load_ps(pSrc);
            __m128 l2=_mm_load_ps(pSrc+mJump);
            __m128 l3=_mm_load_ps(pSrc+2*mJump);
            __m128 l5=_mm_load_ps(pSrc+3*mJump);
            __m128 l1=l0;
            l0=_mm_shuffle_ps(l0,l2,0x88);
            l1=_mm_shuffle_ps(l1,l2,0xdd);
            l2=l0;
            __m128 l6=l1;

            __m128 l4=l3;
            l4=_mm_shuffle_ps(l4,l5,0x88);
            l3=_mm_shuffle_ps(l3,l5,0xdd);

            l2=_mm_shuffle_ps(l2,l4,0x88);
            _mm_store_ps(pDst,l2);
            l0=_mm_shuffle_ps(l0,l4,0xdd);
            _mm_store_ps(pDst+8,l0);
            l6=_mm_shuffle_ps(l6,l3,0x88);
            _mm_store_ps(pDst+4,l6);
            l1=_mm_shuffle_ps(l1,l3,0xdd);
            _mm_store_ps(pDst+12,l1);

            pSrc+=4;
            pDst+=16;
            lWidth-=4;
        }
    }
    else
    {
        while(lWidth>=4)
        {
            __m128 l0=_mm_loadu_ps(pSrc);
            __m128 l2=_mm_loadu_ps(pSrc+mJump);
            __m128 l3=_mm_loadu_ps(pSrc+2*mJump);
            __m128 l5=_mm_loadu_ps(pSrc+3*mJump);
            __m128 l1=l0;
            l0=_mm_shuffle_ps(l0,l2,0x88);
            l1=_mm_shuffle_ps(l1,l2,0xdd);
            l2=l0;
            __m128 l6=l1;

            __m128 l4=l3;
            l4=_mm_shuffle_ps(l4,l5,0x88);
            l3=_mm_shuffle_ps(l3,l5,0xdd);

            l2=_mm_shuffle_ps(l2,l4,0x88);
            _mm_store_ps(pDst,l2);
            l0=_mm_shuffle_ps(l0,l4,0xdd);
            _mm_store_ps(pDst+8,l0);
            l6=_mm_shuffle_ps(l6,l3,0x88);
            _mm_store_ps(pDst+4,l6);
            l1=_mm_shuffle_ps(l1,l3,0xdd);
            _mm_store_ps(pDst+12,l1);

            pSrc+=4;
            pDst+=16;
            lWidth-=4;
        }
        while(lWidth>0)
        {
            *(pDst++)=pSrc[0];
            *(pDst++)=pSrc[mJump];
            *(pDst++)=pSrc[2*mJump];
            *(pDst++)=pSrc[3*mJump];
            pSrc++;
            lWidth--;
        }
    }

}

void CRowSizzler::Sizzle(const unsigned char *pSrc,float *pDst) const
{
    int lWidth=mPart;

    for(int i=0;i<lWidth;i++)
    {
        __m128 l0=_mm_set_ps((float)pSrc[i+3*mJump],(float)pSrc[i+2*mJump],(float)pSrc[i+mJump],(float)pSrc[i]);
        _mm_store_ps(pDst+4*i,l0);
    }
}

void CRowSizzler::DeSizzle(const float *pSrc,unsigned char *pDst) const
{
    int lR1=mPart-mAfter;   //number to extract from first row
    int lS2=lR1-mJump;      //where to start in second row

    for(int i=0;i<lR1;i++)
        pDst[i]=(unsigned char)pSrc[4*i];
    for(int i=lS2;i<lR1;i++)
        pDst[i+mJump]=(unsigned char)pSrc[4*i+1];
    for(int i=lS2;i<lR1;i++)
        pDst[i+2*mJump]=(unsigned char)pSrc[4*i+2];
    for(int i=lS2;i<mPart;i++)
        pDst[i+3*mJump]=(unsigned char)pSrc[4*i+3];
}

void CRowSizzler::DeSizzle(const float *pSrc,float *pDst) const
{
    int lR1=mPart-mAfter;   //number to extract from first row
    int lS2=lR1-mJump;      //where to start in second row

    for(int i=0;i<lR1;i++)
        pDst[i]=pSrc[4*i];
    for(int i=lS2;i<lR1;i++)
        pDst[i+mJump]=pSrc[4*i+1];
    for(int i=lS2;i<lR1;i++)
        pDst[i+2*mJump]=pSrc[4*i+2];
    for(int i=lS2;i<mPart;i++)
        pDst[i+3*mJump]=pSrc[4*i+3];
}

void sizzle_lowpass_x(float *pSrc,float *pDst,const CRowSizzler &pSizzler)
{
    int lW=pSizzler.GetPartW();

    __m128 l0,l1,l2,l3;

    l0=_mm_load_ps(pSrc);
    l1=_mm_add_ps(l0,l0);
    l2=_mm_add_ps(l1,l1);
    l3=_mm_add_ps(l2,l2);

    __m128 lTmp0=l0;
    l0=_mm_load_ps(pSrc+4);
    __m128 lTmp1=l1;
    __m128 lTmp2=l2;
    __m128 lTmp3=l3;
    l1=_mm_add_ps(l0,lTmp0);
    l2=_mm_add_ps(l1,lTmp1);
    l3=_mm_add_ps(l2,lTmp2);

    for (int x=0;x<lW-2;x++)
    {
        lTmp0=l0;
        l0=_mm_load_ps(pSrc+4*(x+2));
        lTmp1=l1;
        lTmp2=l2;
        lTmp3=l3;
        l1=_mm_add_ps(l0,lTmp0);
        l2=_mm_add_ps(l1,lTmp1);
        l3=_mm_add_ps(l2,lTmp2);

        lTmp3=_mm_add_ps(lTmp3,l3);

        _mm_store_ps(pDst+4*x,lTmp3);
    }

    for (int x=lW-2;x<lW;x++)
    {
        lTmp0=l0;
        l0=_mm_load_ps(pSrc+4*(lW-1));
        lTmp1=l1;
        lTmp2=l2;
        lTmp3=l3;
        l1=_mm_add_ps(l0,lTmp0);
        l2=_mm_add_ps(l1,lTmp1);
        l3=_mm_add_ps(l2,lTmp2);

        lTmp3=_mm_add_ps(lTmp3,l3);

        _mm_store_ps(pDst+4*x,lTmp3);
    }
}

void sizzle_first_lowpass_y(float *pSrc, float *pDst1, float *pDst2,
                            float *pDst3, float *pDst4, float *pDst5, const CRowSizzler &pSizzler)
{
    int lW=pSizzler.GetPartW();

    for (int x=0;x<lW;x++)
    {
        __m128 lTmp=_mm_load_ps(pSrc+4*x);
        _mm_store_ps(pDst5+4*x,lTmp);
        lTmp=_mm_add_ps(lTmp,lTmp);
        _mm_store_ps(pDst4+4*x,lTmp);
        lTmp=_mm_add_ps(lTmp,lTmp);
        _mm_store_ps(pDst3+4*x,lTmp);
        lTmp=_mm_add_ps(lTmp,lTmp);
        _mm_store_ps(pDst2+4*x,lTmp);
        lTmp=_mm_add_ps(lTmp,lTmp);
        _mm_store_ps(pDst1+4*x,lTmp);
    }
}

void sizzle_lowpass_y(float *pSrc, float *pDst1, float *pDst2,
                      float *pDst3, float *pDst4, float *pDst5, const CRowSizzler &pSizzler)
{
    int lW=pSizzler.GetPartW();

    for (int x=0;x<lW;x++)
    {
        __m128 lTmp=_mm_load_ps(pSrc+4*x);
        _mm_store_ps(pDst5+4*x,lTmp);
        __m128 lOld=_mm_load_ps(pDst4+4*x);
        lTmp=_mm_add_ps(lTmp,lOld);
        _mm_store_ps(pDst4+4*x,lTmp);
        lOld=_mm_load_ps(pDst3+4*x);
        lTmp=_mm_add_ps(lTmp,lOld);
        _mm_store_ps(pDst3+4*x,lTmp);
        lOld=_mm_load_ps(pDst2+4*x);
        lTmp=_mm_add_ps(lTmp,lOld);
        _mm_store_ps(pDst2+4*x,lTmp);
        lOld=_mm_load_ps(pDst1+4*x);
        lTmp=_mm_add_ps(lTmp,lOld);
        _mm_store_ps(pDst1+4*x,lTmp);
    }
}

void sizzle_highpass(float *pSrc1,float *pSrc2,float *pSrc3,float *pDst,const CRowSizzler &pSizzler)
{
    int lW=pSizzler.GetPartW();

    __m128 lTmp0=_mm_setzero_ps();
    __m128 lTmp1;

    _mm_store_ps(pDst,lTmp0);
    _mm_store_ps(pDst+4,lTmp0);
    _mm_store_ps(pDst+8,lTmp0);

    _mm_store_ps(pDst+12*(lW-1),lTmp0);
    _mm_store_ps(pDst+12*(lW-1)+4,lTmp0);
    _mm_store_ps(pDst+12*(lW-1)+8,lTmp0);

    for(int x=1;x<lW-1;x++)
    {
        lTmp0=_mm_load_ps(pSrc2+4*(x-1));
        lTmp1=_mm_load_ps(pSrc2+4*(x+1));
        __m128 lDX=_mm_sub_ps(lTmp0,lTmp1);
        lTmp0=_mm_load_ps(pSrc1+4*x);
        lTmp1=_mm_load_ps(pSrc3+4*x);
        __m128 lDY=_mm_sub_ps(lTmp0,lTmp1);
        _mm_store_ps(pDst+12*x,_mm_mul_ps(lDX,lDX));
        _mm_store_ps(pDst+12*x+4,_mm_mul_ps(lDX,lDY));
        _mm_store_ps(pDst+12*x+8,_mm_mul_ps(lDY,lDY));
    }
}

void sizzle_sobel(float *pSrc1,float *pSrc2,float *pSrc3,float *pDstXX,float *pDstXY,float *pDstYY,
                  const CRowSizzler &pSizzler)
{
    int lW=pSizzler.GetPartW();

    __m128 lTmp0=_mm_setzero_ps();
    __m128 lTmp1;

    _mm_store_ps(pDstXX,lTmp0);
    _mm_store_ps(pDstXY,lTmp0);
    _mm_store_ps(pDstYY,lTmp0);

    _mm_store_ps(pDstXX+4*(lW-1),lTmp0);
    _mm_store_ps(pDstXY+4*(lW-1),lTmp0);
    _mm_store_ps(pDstYY+4*(lW-1),lTmp0);

    for(int x=1;x<lW-1;x++)
    {
        lTmp0=_mm_load_ps(pSrc2+4*(x-1));
        lTmp1=_mm_load_ps(pSrc2+4*(x+1));
        __m128 lDX=_mm_sub_ps(lTmp0,lTmp1);
        lTmp0=_mm_load_ps(pSrc1+4*x);
        lTmp1=_mm_load_ps(pSrc3+4*x);
        __m128 lDY=_mm_sub_ps(lTmp0,lTmp1);
        _mm_store_ps(pDstXX+4*x,_mm_mul_ps(lDX,lDX));
        _mm_store_ps(pDstXY+4*x,_mm_mul_ps(lDX,lDY));
        _mm_store_ps(pDstYY+4*x,_mm_mul_ps(lDY,lDY));
    }
}

void sizzle_laplace(float *pSrc1,float *pSrc2,float *pSrc3,float *pDst,const CRowSizzler &pSizzler)
{
    int lW=pSizzler.GetPartW();

    for(int i=0;i<4;i++)
    {
        pDst[i]=3.0f*pSrc2[i]-pSrc1[i]-pSrc3[i]-pSrc2[i+4];
        pDst[4*(lW-1)+i]=3.0f*pSrc2[4*(lW-1)+i]-pSrc1[4*(lW-1)+i]-pSrc3[4*(lW-1)+i]-pSrc2[4*(lW-2)+i];
    }

    __m128 l4=_mm_set1_ps(4.0);
    __m128 lTmp0,lTmp1;

    for(int x=1;x<lW-1;x++)
    {
        lTmp0=_mm_load_ps(pSrc2+4*x);
        lTmp0=_mm_mul_ps(lTmp0,l4);
        lTmp1=_mm_load_ps(pSrc2+4*(x-1));
        lTmp0=_mm_sub_ps(lTmp0,lTmp1);
        lTmp1=_mm_load_ps(pSrc2+4*(x+1));
        lTmp0=_mm_sub_ps(lTmp0,lTmp1);
        lTmp1=_mm_load_ps(pSrc1+4*x);
        lTmp0=_mm_sub_ps(lTmp0,lTmp1);
        lTmp1=_mm_load_ps(pSrc3+4*x);
        lTmp0=_mm_sub_ps(lTmp0,lTmp1);
        _mm_store_ps(pDst+4*x,lTmp0);
    }
}

//this should be called for the first pAS rows (with pZero set in the first one)
void sizzle_sum_window_first(float *pSrc, float *pDst,int pWS,bool pZero,const CRowSizzler &pSizzler)
{
    int lWS_2=pWS/2;
    int lW=pSizzler.GetPartW();

    __m128 lTmp0=_mm_setzero_ps();

    if(pZero)
    {
        for(int x=0;x<lW;x++)
        {
            _mm_store_ps(pDst+4*x,lTmp0);
        }
    }

    __m128 lCur=lTmp0;

    for(int x=0;x<pWS;x++)
    {
        lTmp0=_mm_load_ps(pSrc+4*x);
        lCur=_mm_add_ps(lCur,lTmp0);
    }

    __m128 lTmp1;

    lTmp1=_mm_load_ps(pDst+4*lWS_2);
    lTmp1=_mm_add_ps(lTmp1,lCur);
    _mm_store_ps(pDst+4*lWS_2,lTmp1);

    for(int x=pWS;x<lW;x++)
    {
        lTmp1=_mm_load_ps(pDst+4*(x-lWS_2));

        lTmp0=_mm_load_ps(pSrc+4*x);
        lCur=_mm_add_ps(lCur,lTmp0);
        lTmp0=_mm_load_ps(pSrc+4*(x-pWS));
        lCur=_mm_sub_ps(lCur,lTmp0);

        lTmp1=_mm_add_ps(lTmp1,lCur);

        _mm_store_ps(pDst+4*(x-lWS_2),lTmp1);
    }
}

void sizzle_sum_window(float *pSrc1, float *pSrc2, float *pDst, int pWS,const CRowSizzler &pSizzler)
{
    int lWS_2=pWS/2;
    int lW=pSizzler.GetPartW();

    __m128 lUp;
    __m128 lLow;

    lUp=_mm_load_ps(pSrc1);
    lLow=_mm_load_ps(pSrc2);

    for (int x=1;x<pWS;x++)
    {
        lUp=_mm_add_ps(lUp,_mm_load_ps(pSrc1+x*4));
        lLow=_mm_add_ps(lLow,_mm_load_ps(pSrc2+x*4));
    }

    __m128 lTmp1;

    lTmp1=_mm_load_ps(pDst+4*lWS_2);
    lTmp1=_mm_add_ps(lTmp1,lLow);
    lTmp1=_mm_sub_ps(lTmp1,lUp);
    _mm_store_ps(pDst+4*lWS_2,lTmp1);

    __m128 lTmp0;

    for (int x=pWS;x<lW;x++)
    {
        lTmp0=_mm_load_ps(pSrc1+4*x);
        lUp=_mm_add_ps(lUp,lTmp0);
        lTmp0=_mm_load_ps(pSrc1+4*(x-pWS));
        lUp=_mm_sub_ps(lUp,lTmp0);

        lTmp0=_mm_load_ps(pSrc2+4*x);
        lLow=_mm_add_ps(lLow,lTmp0);
        lTmp0=_mm_load_ps(pSrc2+4*(x-pWS));
        lLow=_mm_sub_ps(lLow,lTmp0);

        lTmp1=_mm_load_ps(pDst+4*(x-lWS_2));

        lTmp1=_mm_add_ps(lTmp1,lLow);
        lTmp1=_mm_sub_ps(lTmp1,lUp);

        _mm_store_ps(pDst+4*(x-lWS_2),lTmp1);
    }
}

void sizzle_harris(float *pSrcXX,float *pSrcXY,float *pSrcYY,float *pDst,float pHarrFac,const CRowSizzler &pSizzler)
{
    int lW=pSizzler.GetPartW();
    __m128 lHarrFac=_mm_set1_ps(pHarrFac);

    __m128 lTmp1,lTmp2,lTmp3;
    __m128 lDet,lTra;

    for(int x=0;x<lW;x++)
    {
        lTmp1=_mm_load_ps(pSrcXX+4*x);
        lTmp2=_mm_load_ps(pSrcXY+4*x);
        lTmp3=_mm_load_ps(pSrcYY+4*x);

        lDet=_mm_mul_ps(lTmp1,lTmp3);
        lTmp2=_mm_mul_ps(lTmp2,lTmp2);
        lDet=_mm_sub_ps(lDet,lTmp2);
        lTra=_mm_add_ps(lTmp1,lTmp3);

        lTra=_mm_mul_ps(lTra,lTra);
        lTra=_mm_mul_ps(lTra,lHarrFac);

        lDet=_mm_sub_ps(lDet,lTra);

        _mm_store_ps(pDst+4*x,lDet);
    }
}

void sizzle_harris_find_peak(float *pSrc1,float *pSrc2,float *pSrc3,float *pDst,
                             float pThresh,int pY,std::vector<std::pair<CPoint_d,float>> &pPeaks,const CRowSizzler &pSizzler)
{
    int lW=pSizzler.GetPartW();
    int lJump=pSizzler.GetJump();
    __m128 lTmp0=_mm_setzero_ps();

    for (int x=0;x<7;x++)
        _mm_store_ps(pDst+4*x,lTmp0);

    __m128 lThresh=_mm_set1_ps(pThresh);
    __m128 lMax;

    for (int x=6;x<lW-7;x++)
    {
        lMax=_mm_max_ps(lThresh,_mm_load_ps(pSrc1+4*x));
        lMax=_mm_max_ps(lMax,_mm_load_ps(pSrc2+4*x));
        lMax=_mm_max_ps(lMax,_mm_load_ps(pSrc3+4*x));
        _mm_store_ps(pDst+4*x+4,lMax);
    }

    for (int x=7;x<lW-8;x++)
    {
        lMax=_mm_max_ps(_mm_load_ps(pDst+4*x),_mm_load_ps(pDst+4*x+4));
        lMax=_mm_max_ps(lMax,_mm_load_ps(pDst+4*x+8));

        _mm_store_ps(pDst+4*x,lMax);

        for (int i=0;i<4;i++)
        {
            if(pDst[4*x+i]==pSrc2[4*x+i])
            {
                int lXP=i*lJump+x;
                int j = 4*x+i;
                CPoint_d lPos;
                if(pSrc2[j-4]==pSrc2[j+4])
                {
                    lPos.x=lXP;
                    lPos.y=pY;
                }
                else
                {
                    lPos.x=lXP+0.5f*(pSrc2[j-4]-pSrc2[j+4])/
                            (pSrc2[j-4]+pSrc2[j+4]-2.0f*pSrc2[j]);
                    lPos.y=pY+0.5f*(pSrc1[j]-pSrc3[j])/
                            (pSrc1[j]+pSrc3[j]-2.0f*pSrc2[j]);
                }

                pPeaks.push_back(std::make_pair(lPos,pSrc2[j]));
            }

            pDst[4*x+i]=(pDst[4*x+i]>pSrc2[4*x+i])?0.0f:1.0f;
        }
    }

    for (int x=lW-8;x<lW;x++)
        _mm_store_ps(pDst+4*x,lTmp0);
}

/*namespace img*/ } /*namespace buola*/ }
